package it.gius.processing.gjkTest;


import java.awt.Color;

import org.jbox2d.common.Vec2;

import processing.core.PApplet;
import it.gius.pe.SystemCostants;
import it.gius.pe.Shape.Polygon;
import it.gius.pe.algorithm.gjk.DistanceSolution;
import it.gius.pe.algorithm.gjk.GjkUtilsFunctions;
import it.gius.pe.algorithm.gjk.Simplex;
import it.gius.pe.algorithm.gjk.SimplexSolution;
import it.gius.pe.mains.MainMovingTest;
import it.gius.processing.util.MyAbstractPApplet;

@SuppressWarnings("serial")
public class GjkPolyPolyStepToStep extends MyAbstractPApplet {



	private Polygon[] arrayPoly = null;
	GjkUtilsFunctions utils = new GjkUtilsFunctions();

	private Simplex simplex1 = new Simplex(Simplex.LARGER_2D_SIMPLEX);
	private Simplex simplex2 = new Simplex(Simplex.LARGER_2D_SIMPLEX);
	private Simplex simplexZ = new Simplex(Simplex.LARGER_2D_SIMPLEX);

	public GjkPolyPolyStepToStep(Polygon[] arrayPoly) {
		this.arrayPoly = arrayPoly;
	}

	public void setup(){


		simplex1.vs[0].set(arrayPoly[0].getVertex(0));
		simplex1.currentDim = Simplex.IS_0_SIMPLEX;
		
		simplex2.vs[0].set(arrayPoly[1].getVertex(0));
		simplex2.currentDim = Simplex.IS_0_SIMPLEX;
		
		simplexZ.vs[0] = simplex2.vs[0].sub(simplex1.vs[0]);
		simplexZ.currentDim = Simplex.IS_0_SIMPLEX; 

		size(MainMovingTest.sizeX,MainMovingTest.sizeY);
		smooth();
		
		//PFont font = loadFont("Times New Roman");
		/*PFont.loadFonts();
		String[] fonts = PFont.list();
		PFont font = loadFont(fonts[0]);
		textFont(font);*/ 

	}
	
	@Override
	public void keyPressed() {
		super.keyPressed();
		
		if(key == 'a')
		{
			redraw();
		}
	}
	
	
	private boolean done = false;
	


	public void draw()
	{

		if(!goStop.isGo())
			noLoop();



		if(done)
		{
			println("DONE");
			return;
		}

		background(255);
			/*for(int i=0;i < arrayPoly.length; i++)
				arrayPoly[i].move();*/




			for(int i= 0;i < arrayPoly.length; i++)
			{
				fill(0,0,210);
				stroke(10);
				drawPoly(arrayPoly[i],new Color(0,0,210));
				fill(0);
				text(i+1,arrayPoly[i].center.x,arrayPoly[i].center.y);
			}
			
			//stroke(210,0,0);
			drawSimplex(simplex1, new Color(210,0,0), new Color(210,0,0));
			drawSimplex(simplex2, new Color(210,0,0), new Color(210,0,0));
			
			DistanceSolution sol = new DistanceSolution();

	
			
			Simplex simplex1Temp = new Simplex(Simplex.LARGER_2D_SIMPLEX);
			
		
			Simplex simplex2Temp = new Simplex(Simplex.LARGER_2D_SIMPLEX);
			
		

			SimplexSolution npSol = new SimplexSolution();

			Vec2 d = null;
			Vec2 currSol = new Vec2();
			Vec2 origin = SystemCostants.ORIGIN2D;
			
			Vec2 p1Temp,p2Temp;
			
			float dim = (float)(arrayPoly[0].getVerticesCount() + arrayPoly[1].getVerticesCount());
			
			//int max = (int)(dim*dim*dim/6 + dim*dim/2 + dim);
			int max = (int)(dim*dim);
			
			
			//for(int iteration=0; iteration<max; iteration++)
			for(int i=0; i<simplexZ.currentDim;i++)
				System.out.println("simplexZ"+i+": " +simplexZ.vs[i]);
				
				utils.nearestPointSimplex2D(simplexZ, origin, npSol);

				npSol.getSolutionPoint(currSol);
				

				float g = utils.gOrigin(currSol, arrayPoly[0], arrayPoly[1]);
				
				System.out.println("gOrigin: " +g);
				
				p1Temp = getPointSolution(simplex1, simplexZ, npSol);
				p2Temp = getPointSolution(simplex2, simplexZ, npSol);

				d = origin.sub(currSol); 
				if(g >= -Math.sqrt(SystemCostants.EPSILON) && g<= Math.sqrt(SystemCostants.EPSILON))
				{
					sol.p1 = p1Temp;
					sol.p2=  p2Temp;
					sol.distance = d.length();
					done = true;
				}

				System.out.println("npSol.numVerticesUsed: " + npSol.numVerticesUsed);
				//internal point: compenetration
				if(npSol.numVerticesUsed == Simplex.LARGER_2D_SIMPLEX)
				{
					sol.p1 = p1Temp;
					sol.p2 = p2Temp;
					//TODO distanza negativa tale che  d(Ki,Kj) = -min{|z| : z appartiene a boundary di K}
					sol.distance = 0;
					done = true;
				}

				int index2 = utils.supportPointIndex(arrayPoly[1], d);
				int index1 = utils.supportPointIndex(arrayPoly[0], currSol);
				
				Vec2 zSupport = arrayPoly[1].getVertex(index2).sub(arrayPoly[0].getVertex(index1));

				boolean supportAlreadyIn = false;

				for(int i =0; i< npSol.numVerticesUsed;i++)
				{
					int j;
					for( j=0; !simplexZ.vs[j].equals(npSol.verticesInSolution[i]);j++);
					
					simplex1Temp.vs[i] = simplex1.vs[j];
					simplex2Temp.vs[i] = simplex2.vs[j];
					/*if(simplex.vs[i].equals(poly.getVertex(index)))
						supportAlreadyIn = true;*/
				}
				
				for(int i =0; i< npSol.numVerticesUsed;i++)
				{
					simplex1.vs[i] = simplex1Temp.vs[i];
					simplex2.vs[i] = simplex2Temp.vs[i];
					
					simplexZ.vs[i] = simplex2.vs[i].sub(simplex1.vs[i]);
					
					if(simplexZ.vs[i].equals(zSupport))
						supportAlreadyIn = true;
				}

				simplex1.currentDim = npSol.numVerticesUsed;
				simplex2.currentDim = npSol.numVerticesUsed;
				simplexZ.currentDim = npSol.numVerticesUsed;

				if(supportAlreadyIn)
				{
					sol.p1 = p1Temp;
					sol.p2 = p2Temp;
					sol.distance = d.length();
					//done = true;
				}

				if(simplexZ.currentDim < Simplex.LARGER_2D_SIMPLEX)
				{
					simplex1.vs[npSol.numVerticesUsed] = arrayPoly[0].getVertex(index1);
					simplex1.currentDim++;
					
					simplex2.vs[npSol.numVerticesUsed] = arrayPoly[1].getVertex(index2);
					simplex2.currentDim++;
					
					simplexZ.vs[npSol.numVerticesUsed] = zSupport;
					simplexZ.currentDim++;
				}		

				//stroke(0,210,0);
				drawSimplex(simplex1,new Color(0,210,0),new Color(0,210,0));
				drawSimplex(simplex2,new Color(0,210,0),new Color(0,210,0));
				
				
				stroke(210,210,0);
				line(p2Temp.x,p2Temp.y,p1Temp.x,p1Temp.y);


	}

	
	private void drawSimplex(Simplex simplex,Color fillColor, Color strokeColor)
	{
		
		//if(simplex.currentDim == Simplex.IS_0_SIMPLEX)
		for(int i=0; i < simplex.currentDim; i++)
		{
			fill(fillColor.getRed(),fillColor.getGreen(),fillColor.getBlue());
			ellipse(simplex.vs[i].x,simplex.vs[i].y,4,4);
		}
		
		noFill();
		stroke(strokeColor.getRed(),strokeColor.getGreen(),strokeColor.getBlue());
		beginShape();
		for(int i = 0; i < simplex.currentDim; i++)
		{
			vertex(simplex.vs[i].x,simplex.vs[i].y);
		}
		endShape(PApplet.CLOSE);
	}

private  Vec2 getPointSolution(Simplex simplex12, Simplex simplexZ, SimplexSolution npS)
{
	Vec2 result = new Vec2();
	result.setZero();
	for(int i=0; i<npS.numVerticesUsed;i++)
	{
		int j;
		for( j=0; !simplexZ.vs[j].equals(npS.verticesInSolution[i]);j++);
		
		result.addLocal(simplex12.vs[j].mul(npS.coefficients[i]));
		
			
	}
	
	return result;
}

	private void drawPoly(Polygon poly, Color fillColor)
	{
		fill(0);
		for(int i=0; i< poly.getVerticesCount(); i++)
		{
			Vec2 v = poly.getVertex(i);
			text(i,v.x-3,v.y);
		}
		
		
		fill(fillColor.getRed(),fillColor.getGreen(),fillColor.getBlue());
		beginShape();
		for(int i=0; i< poly.getVerticesCount(); i++)
		{
			Vec2 v = poly.getVertex(i);
			vertex(v.x,v.y);
		}
		endShape(PApplet.CLOSE);

	}
}
